import rclpy
from rclpy.node import Node
from face_interfaces.srv import FaceDetect
import cv2
# 获取功能包路径
from ament_index_python.packages import get_package_share_directory 
from cv_bridge import CvBridge
import os
from rcl_interfaces.srv import SetParameters
from rcl_interfaces.msg import Parameter,ParameterValue


class FaceDetectClientNode(Node):

    def __init__(self):
        super().__init__("face_detect_client_node")
        self.client_=self.create_client(FaceDetect,"face_detect")
        self.bridge=CvBridge()
        self.image_path=os.path.join(get_package_share_directory("demo_python_service")+"/img/face_sr.jpeg")
        self.get_logger().info("人脸检测客户端启动")
        self.cv_image=cv2.imread(self.image_path)

    def call_set_parameters(self,parameters):
        '''
        修改参数服务
        '''
        # 创建客户端，等待上线
        update_param=self.create_client(SetParameters,'/face_detect_node/set_parameters')
        while self.client_.wait_for_service(timeout_sec=0.1) is False:
            self.get_logger().info("wait service")
        # 创建request
        request=SetParameters.Request()
        request.parameters=parameters
         # 发送请求
        future=update_param.call_async(request)
        # 等待返回结果
        rclpy.spin_until_future_complete(self,future)

        return request

    def update_detect_model(self,model='hog'):
        '''更新服务端参数'''
        # 创建参数对象
        param=Parameter()
        param.name='model'
        # 赋值
        param_value=ParameterValue()
        param_value.type=ParameterValue.PARAMETER_STRING
        param_value.string_value=model
        param.value=param_value
        # 请求参数
        response=self.call_set_parameters([param])
        for result in response.result:
            self.get_logger().info(f'请求参数结果:{result.successful}{result.reason}')



    def send_request(self):
        #等待服务    
        while self.client_.wait_for_service(timeout_sec=0.1) is False:
            self.get_logger().info("wait service")
        # 构建Request
        request=FaceDetect.Request()
        request.image=self.bridge.cv2_to_imgmsg(self.cv_image)
        # 发送请求
        future=self.client_.call_async(request)

        # 等待返回结果
        rclpy.spin_until_future_complete(self,future)
        
        response=future.result()
        self.get_logger().info(f"检测到人脸{response.number}个，耗时{response.use_time}s")
        self.show_response(response)

    def show_response(self,response):
        for i in range(response.number):
            top=response.top[i]
            right=response.right[i]
            left=response.left[i]
            bottom=response.bottom[i]
            cv2.rectangle(self.cv_image,(right,top),(left,bottom),(255,0,0),4)

        # cv2.imshow("result",self.cv_image)
        # cv2.waitKey(0)



def main():
    rclpy.init()
    node=FaceDetectClientNode()
    node.update_detect_model('hog')
    node.send_request()
    node.update_detect_model('cnn')
    rclpy.spin(node)
    rclpy.shutdown()
